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ABSTRACT 



Tracking multiple targets in a cluttered environment is extremely difficult. 
Traditional approaches generally use simple techniques that combine gating with some 
form of nearest neighbor association to reduce the effects of clutter. When clutter 
densities increase, these traditional algorithms fail to perform well. To counter this 
problem, the multi-hypothesis tracking (MHT) algorithm was developed. This approach 
enumerates almost every conceivable combination of measurements to determine the 
most likely tracks. This process quickly becomes very complex and requires vast 
amounts of memory in order to store all of the possible tracks. 

To avoid this complexity, more sophisticated single hypothesis data association 
techniques have been developed, such as the probabilistic data association filter (PDAF). 
These algorithms have enjoyed some success, but do not take advantage of any future 
data to help clarify ambiguous situations. 

On the other hand, the probabilistic multi-hypothesis tracking (PMHT) algorithm, 
proposed by Streit and Luginbuhl in 1995, attempts to use the best aspects of the MHT 
and the PDAF. In the PMHT algorithm, data is processed in batches, thereby using 
information from before and after each measurement to determine the likelihood of each 
measurement-to-track association. Furthermore, like the PDAF, it does not attempt to 
make hard assignments or enumerate all possible combinations, but instead associates 
each measurement with each track based upon its probability of association. 
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Actual performance and initialization of the PMHT algorithm in the presence of 
significant clutter has not been adequately researched. This study focuses on the 
performance of the PMHT algorithm in dense clutter and the initialization thereof. In 
addition, the effectiveness of measurement attribute data is analyzed, especially as it 
relates to algorithm initialization. Further, it compares the performance of this algorithm 
to the nearest neighbor, MHT, and PDAF. 
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I. INTRODUCTION 



A. BACKGROUND 

Tracking multiple targets in a cluttered environment, such as is found in littoral 
waters, is an extremely difficult task. This is due to the added noise which is caused by 
the closeness of the ocean floor to the surface. Furthermore, in littoral waters, there are 
quite often various man-made structures, which can cause the addition of false target 
sonar returns. In this environment, targets typically operate at speeds between 2-10 
knots. In order to maintain steerage control, submarines find it difficult to operate below 
this minimum velocity, and above 10 knots, diesel submarines will produce an obvious 
amount of noise. 

Traditional approaches used to solve the cluttered environment tracking problem 
have typically employed simple techniques to determine what is a true measurement from 
a target and what is not. They accomplish this by a combination of gating (discarding 
measurements) and some form of nearest neighbor association (picking the closest 
measurement to the current target position estimate, where “closest” is defined by a 
weighted distance). In the open ocean, where the water is deep, traditional approaches 
perform well because the problem of dense clutter is not encountered. However, in 
littoral waters, where clutter densities increase, these traditional algorithms fail to yield 
reliable results. In order to solve the clutter problem, two algorithms were developed — 
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the multi-hypothesis tracking (MHT) algorithm and the probabilistic data association 
filter (PDAF). 

1. The Multi-Hypothesis Tracker (MHT) 

The MHT [Ref. 5] enumerates almost all of the possible combinations of 
measurement-to-track assignments. Then from all of these possibilities, the most likely is 
selected as the best estimate of the track. The problem quickly becomes extremely 
complex as the data combinations are growing exponentially as each new measurement 
batch is received. This requires a huge amount of memory and computing power. 
Furthermore, as the number of possibilities increase, some form of pruning must be done 
in order to keep the number of hypotheses within limits. 

2. The Probabilistic Data Association Filter (PDAF) 

On the other hand, the PDAF [Ref. 4] does not make hard measurement-to-track 
assignments, but rather weights each measurement based upon its likelihood of 
association with a track. This algorithm has some advantages in its simplicity, especially 
in computational and storage costs. However, it only gets one chance to weight the data 
correctly. Therefore, this algorithm does not take advantage of any future data before 
making a decision on the most likely true measurement. 



B. A NEW APPROACH 

In 1995, Streit and Luginbuhl of the Naval Undersea Warfare Center proposed a 
new algorithm called the probabilistic multi -hypothesis tracking (PMHT) algorithm [Ref. 
1]. Like the MHT. this algorithm processes data in batches, thereby giving it the 






advantage of future data before decisions are made. However, the PMHT does not 
attempt to enumerate all possible combinations, but rather weights the measurements 
based on the likelihood of each measurement being the true measurement. Therefore, like 
the PDAF, this new algorithm employs an empirical, Bayesian data association to score 
the measurements in order to determine the likely true measurement centroid. This 
technique can be significantly faster than the MHT, but will require more time to 
compute than the PDAF. This research focuses on two primary areas of the PMHT that 
have yet to be studied carefully, that is, the actual performance of the algorithm in the 
presence of dense clutter and the initialization thereof. 

Furthermore, this thesis also addresses the performance of the PMHT as compared 
to the traditional tracking algorithms — the MHT and PDAF. In addition, measurement 
attribute data is explored in conjunction with the PMHT algorithm. 

C. THESIS OUTLINE 

This thesis is divided into the following chapters: Chapter II describes the theory 
and derivation behind the PMHT algorithm. Chapter III lays out the explicit algorithm as 
it has been implemented in this study. Chapter IV covers the difficulty of initializing this 
algorithm in the presence of clutter, and how the initialization was eventually 
accomplished. Chapter V shows the results of this implementation of the PMHT 
algorithm and compares these results to other tracking algorithms, i.e.. the nearest 
neighbor. PDAF, and MHT. Portions of these results were published and presented at the 
Asilomar Conference on Signals, Systems, and Computers in November of 1996 [Ref. 2]. 



Finally, Chapter VI summarizes this study and offers suggestions about areas in which 
further research might be conducted. 
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II. THEORETICAL BASIS OF THE PMHT ALGORITHM 



A. TARGET MOTION MODEL 



In this research, measurements are obtained from a sensor in batches at a set time 
interval. The PMHT algorithm takes all of the received measurements and computes an 
optimal estimate for each target track. The targets are assumed to be independent with 
linear Gaussian statistics of the following form: 

X , + A/ = 0x , + w , (2.1) 

where x is the state-space vector containing the x position, x velocity, y position, and y 
velocity, respectively. <t> is the following discrete state-space matrix: 



1 At 
0 1 
0 0 
0 0 



0 0 
0 0 
1 At 
0 1 



( 2 . 2 ) 



Further, w, is white Gaussian noise with known covariance matrix Q, which is given by: 



At 3 / 3 
At 2 / 2 
0 
0 



At 2 / 2 0 

At 0 

0 At 2 / 3 

0 At 2 / 2 



0 

0 

At 2 / 2 

At 



(2.3) 



where At is the time in between scans, and q is a parameter which reflects the 
maneuvering behavior of the target. This parameter is used to adjust the performance of 
the Kalman Filter. Usually for fairly straight tracks, q is set to a small but nonzero value 
in order to prevent covariance collapse in the Kalman algorithm. 
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B. MEASUREMENT MODEL 



The measurement model in this research assumes that a sensor returns range and 
bearing information which contains additive Gaussian noise. Therefore, each 
measurement pair is of the following form: 



= 



<Pr, 



+ e. 



(2.4) 



where the subscript r denotes the index for measurements within a scan (r = 1 and 
the subscript t specifies the discrete time index ( t = 0,...,T). Hence, there are nj total 
measurements taken at time t, and there are T total scan times in the scenario. The error 



vector is additive Gaussian noise with zero mean and covariance given by: 



R = 



0 



(2.5) 



For this research, a r = 100 meters and = 3 degrees was assumed. In this case, with the 
coordinates in polar form, the measurement covariance matrix is the same for all 
measurements at all time scans. However, if the measurements were in another 
coordinate system (e.g., Cartesian) then each measurement would have its own 



covariance matrix. 



C. COORDINATE CONVERSION 

The Kalman Smoother requires a linear state equation and a linear measurement 
equation. The bearing measurement in polar coordinates is nonlinear. Therefore, in order 
to use these measurements in the classical Kalman Smoother, it is necessary to convert 
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them into Cartesian coordinates. Lerro and Bar-Shalom have demonstrated that 
converting range-bearing measurements into Cartesian space prior to implementing the 
Kalman algorithm is superior to utilizing the raw range-bearing measurements directly in 
the Extended Kalman algorithm [Ref. 3]. 

Lerro and Bar-Shalom recommend converting the measurements to Cartesian 
coordinates using their “debiased” equations. These equations convert both the 
measurement itself and its associated covariance matrix. The following are the debiased 
measurement conversions: 



r„cos(^)[l-(e- CT J-e^ /2 )] 
r„ sin(^,)[l-(e'^ -e'^ 2 )] 

Furthermore, the corresponding covariance matrix is given by: 




R 



it 



■^11 ^12 
R n R 22 



( 2 . 6 ) 



(2.7) 



where, 



R u = r^e' 2a * [cos 2 ^„ (cosh2<xJ - coshcrj ) + sin 2 (f> n (sinh2crj - sinhcxj )] 

+ cr 2 e' 2CT<> [cos 2 <j> rl (2cosh2crJ - coshcxj ) + sin 2 <f> rt (2 sinh 2<jJ - sinhcxj )] 
*12 = sin^cos^e' 4 ^ [cr; + (r f 2 + <j 2 )(1 - e ff; )] 

R 22 = r r 2 e' 2<T * [sin 2 $ n (cosh2crJ - cosher 2 ) + cos 2 (j> rl (sinh2crj - sinherj )] 

+ cr 2 e' 2ff * [sin 2 ^ r , (2cosh2crJ - coshcj ) + cos 2 (/>„ (2sinh2crJ - sinhexj )] 



With these equations, there is a different covariance matrix for each measurement at each 
different time scan. 
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D. ITERATIONS 



Each iteration of the PMHT algorithm begins with a set of track position estimates 
and a set of measurement probabilities. Then the weights are computed and centroids 
formed. These centroids are then used in the Kalman Smoother to update the track 
position estimates. With the new estimates, a new set of weights is computed. These 
weights will be similar to the previous weights, but they will be different because of the 
new estimates. After some iterations, the weights will converge. When convergence is 
reached, the current estimate is theoretically the optimal estimate for a given track. 

E. WEIGHTING THE MEASUREMENTS 

In this research, two different models were used to assign weights to 
measurements. The first is the target track model, which uses a normal distribution to 
assign weights to measurements. The second is the clutter model, which uses a constant 
value to assign weights to the measurements. 

1. Track Model 

Given the measurements and an initial estimate at each time scan, the PMHT uses 
a normal distribution between the estimate and each measurement to determine the value 
of the weight assigned to each measurement in a given scan. This weight specifies the 
likelihood that this measurement belongs to a particular track model. From these weights 
and measurements, a centroid is computed for each track model. The centroid is 
calculated by simply multiplying each measurement by its associated weight and then 
summing all of these together. 
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The weight calculated for each measurement is conditioned on the position of the 
estimate and its covariance matrix. If a given measurement is far from the estimate, it 
will get a low weight. On the other hand, if a measurement is near the estimate, it will 
receive a high weight. 

2. Clutter Model 

Since uniform clutter is assumed for this research, the clutter weight is equal to a 
constant, which can be adjusted. The clutter weight was initially determined by 
calculating the area of interest for which measurements will be returned. The inverse of 
this area was then used as the starting point for the clutter weight value. For optimal 
performance, a clutter weight value an order of magnitude less than this value was used. 

F. KALMAN SMOOTHING 

The estimates at each scan are linked together by the Kalman Smoother. The 
centroids at each time scan are used as the “measurements” in the Kalman Smoother. 

This produces a new set of estimates for each track model. The Kalman Smoother is used 
so that all estimates are updated using all the available information from before and after 
each time scan. This produces the best estimate possible at each time scan. 

If the conventional Kalman is used, then each measurement will have its own 
corresponding covariance matrix. This covariance matrix is used both in the Kalman 
Smoothing step, as well as in the weighting step. This not only complicates the 
computations and requires more memory storage, but also causes both the position 
estimate and its associated covariance matrix to have to converge in the iteration process. 
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On the other hand, if the Extended Kalman is used, the same covariance matrix is 



used for all measurements at all time increments not only in computing the weights, but 
also in the smoothing process. This allows simplification of the computations, and 
during the iteration process, only the position estimate is being refined and convergence 
is more easily achieved. 
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III. THE PMHT ALGORITHM IN EXPLICIT FORM 



A. THE BASIC ALGORITHM 

This algorithm is taken from Streit and Luginbuhl [Ref. 1 ] using the linear 
Gaussian case. In section B, I will discuss the modifications which were made to the 
original algorithm, initially. Then in section C, I will cover the modifications, which 
further improved the performance of the algorithm. 

1. Initialization 

Measurement probabilities, n (0) = } must be assigned so that > 0 . It is 

not critical what values are assigned to these measurement probabilities because in the 
first iteration they will be recalculated. Moreover, they do not have an adverse effect 

before they are recomputed. The values specify the estimated probability that a 
measurement at scan t is assigned to target model m after i iterations of the PMHT 
algorithm. 

An initial target state (xj^ ,x, ( ^ ,..., x^ ) for each time increment and each of the 
M target models must be assigned. My experience has shown that these initial estimates 
must be fairly accurate to ensure that the algorithm performs satisfactorily as clutter 
densities increase. 

In this paper, m specifies the target model (m = t specifies the discrete 

time index ( t = 0,...,7); r specifies the index for measurements within a scan (r = 1,. 
and the superscript i specifies the iteration index (z = 0,1,...). 
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2. Computation of the Weights 

For every target and measurement combination at each scan, a weight is 
computed. The value of the likelihood function (assuming normal distribution) evaluated 
at the error between the current estimated position and each measurement is used for the 
weight: 



w, 



*('+') _ 



exp 



(-i' 



r(')T 



S' 1 Z* 0 ) 

" tm i mir } 



27i^/det(£,J 



(3.1) 



w, 



(»+1) 



W 



►(i+1) 



Ztt 0) 

s-\ 



Is 



w 



*(»> 1) 



(3.2) 



where, 



(/) 



z {,) = z — z 1 =z -C x 



(3-3) 



is the error between the current estimate and a measurement. Further, £ is the weighting 
matrix defined as: 



£,„, =C p CL+R, 

//?/ Ini lm tm t 



(3-4) 



Here P /m is the covariance matrix associated with the target state estimate, and C, m is the 



measurement matrix defined as: 



C =C = 



10 0 0 
0 0 10 



(3.5) 



for the standard Kalman algorithm. 
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3. Calculation of the Measurement Centroids 

First, the mean measurement weight for each target model m at time t is defined 



i? (/+1) = - Sw (,+1) 

J v mt n * mtt 



Next, the measurement centroid is computed as: 



(3.6) 






1 



n w ( ' +l) 

i mi 




(3.7) 



This measurement centroid will be used in the Kalman Smoothing step below. 

4. Target Measurement Probabilities Update 

The next step before the Kalman Smoother is to update the target measurement 
probabilites. This is computed as: 



= vr* ,+1 ' 

mi nil /l 'mi 



(3.8) 



5. Target State Sequences Update 

The target state sequences are updated via the Kalman Smoother using the 
measurement centroids as the inputs. First, the intermediate variables of the forward 
recursion are initialized as: 



y*> = < (3-9) 

P»=Po» (3.10) 

Here with these dummy variables, the model m and iteration index i have been 
suppressed for notational simplicity. The forward recursion is defined for t = 0,1,.. .,7-1 
as: 
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p, + .i, =^1,^ + Q 



(3.11) 




R, +1 , m ]'‘ (3-12) 




(3-13) 



®y,i, + G , + i[ 



^/ + l,OT 






(3-14) 



Then the updated target state estimate for model m at time t is: 




(3.15) 



and the updated target state estimates for t = 7’-l,...,l,0 are computed via the backward 
recursion as: 



The equations in this subsection make up a bank of M Kalman Smoothers which can be 
run in parallel, although these filters are not independent because they are linked by the 
weights. In these equations, <t> is the same as was defined in (Eqn. 2.2). 

Therefore, a block diagram of the basic algorithm is shown below in Figure 3.1. 
The convergence block will be discussed in the next section. 




(3.16) 



iterate 




x 0 and n J ^ weights 



Initial \ Compute 



\Jz. 




Form 



7 1 Smoother 



Kalman 




Figure 3.1. Basic PMHT 
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B. INITIAL MODIFICATIONS 

Several modifications and additions are necessary in order for the PMHT 
algorithm to begin working. 

1. Clutter Weight Model 

First, there needs to be a model for clutter weights. Since uniform clutter is 
assumed for this problem, I used a clutter weight equal to a constant, which could be 
adjusted. Therefore, 

p o-n) 

was used for the clutter model and (Eqn. 3.1) was used for target track models. 

2. Convergence Criteria 

The basic algorithm does not specifically state how convergence is to be 
determined. In this research, convergence was measured by the rvalues. It would also 
be possible to test convergence through the weights. However, both approaches yield 
similar results and the rvalues are quicker to sum and compare. Therefore, initially 
convergence was achieved when 

Z-L]*" -xi:' , \<Kc> 0 (3.18) 

The parameter Kc is adjusted for optimal performance. If this number is set too 
high, then the algorithm will stop before it has fully converged to its best solution. On 
the other hand, if this number is set too low, then the algorithm might never be able to 
meet this criteria. Initially Kc was set to 10 -4 . Iterations are allowed to continue until 
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convergence is reached or the maximum limit is exceeded (100 iterations). I will refer to 
this convergence parameter as the stopping criteria in the sections below. 



3. 



Measurement Covariance 



Using the basic algorithm in Cartesian Coordinates requires that each 
measurement have its own associated covariance matrix. For the weighting equation 
(Eqn. 3.1), it is possible to use each measurement’s associated covariance matrix. 
However, during the Kalman Smoothing step, a covariance matrix is needed for each 
track at each time scan. Furthermore, since the measurement centroids are mixtures of 
the received measurements, there is not just one matrix for each track model at each scan. 
The following variations were investigated during this research: 

a. Weighted Covariance 

For this calculation, a weighted covariance matrix is obtained by a similar 
process as is used to obtain the measurement centroids. 
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(3.19) 



b. Closest Measurement 

The covariance matrix associated with the measurement which is closest to 
the current estimate is used. 



R»’ = R„ (3.20) 

c. Covariance of the Estimate 

Here, the covariance matrix is computed from the current estimated 
position by using the debiased equations (Eqn. 2.7). 
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r!;’ = R rti’) 



(3-21) 



Of these variations, the estimated covariance (Eqn. 3.21) has shown the 
most robustness and has provided the best overall results. Furthermore, it has been found 
that using this estimated covariance matrix not only in the Kalman Smoother (Eqn. 3.12), 
but also in the computation of the weights (Eqn. 3.4), provided even better results. 

4. Distant Clutter 

It was found that if a measurement point was too far from the estimated position 
then the weight calculated was extremely small, and numerical instabilities would be 
encountered. Therefore, on a suggestion from Dr. Streit, any measurements beyond a 
Chi-squared cut-off value of 0.995 from the estimated track position have their weight set 
to a low constant value of 10‘ 20 , rather than using the actual weight from (Eqn. 3.1). 

5. Five Scan Batches 

The initialization process (to be discussed in Chapter IV) produces the initial 
estimates for the first five time scans. The PMHT algorithm is run on these five time 
scans until convergence is reached. Then the next five time scans are predicted, and the 
algorithm runs over the now 1 0 time scans, and so forth. This not only provides a more 
realistic approach to what might actually be implemented, but also allows the algorithm 
to only have to sort out five new points at a time. 

6. Speed Limit 

Since the targets of concern in this research have speeds of 2-10 knots, a 10 knot 
maximum speed is imposed when predicting ahead. The actual target estimates produced 
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by the PMHT algorithm are not limited and can converge to an estimate with any 
velocity. 

Therefore, a block diagram with these modifications is shown in Figure 3.2. 



iterate 




C. FURTHER MODIFICATIONS 

The above modifications were sufficient in order for the PMHT algorithm to work 
well. With these modifications, the PMHT algorithm was able to perform better than the 
PDAF at clutter densities up to 3.33xl0‘ 2 clutter points per square kilometer. This value 
for the clutter density appears to be low. However, the standard deviation for the range is 
+/-100 meters, and the standard deviation for the bearing is +/- 3 degrees. Therefore, with 
these values in the measurement covariance matrix, this clutter density is fairly 
significant. 
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On the other hand, in order to increase the performance of the algorithm further, 
the following modifications were also implemented and found to be effective, especially 
for the higher clutter densities. 

1. Clutter Weight Model Inflation 

Adjusting the clutter weight model parameter r after the algorithm has sorted out 
the first 10 points has proven beneficial. That is, for the first 10 points, p is set to a value 
of 10' 12 , and then it is increased to a value of 10' 10 for the rest of the batches. 

2. Extended Kalman 

The numerical complications involved with the measurement covariance matrix 
led to trying the Extended Kalman algorithm. As has been discussed, the classical 
Kalman requires a different covariance matrix for each different point in the Cartesian 
Coordinates. On the other hand, if the Extended Kalman is used, one measurement 
covariance matrix is used for all models at all time scans. Using this approach has shown 
some significant advantages which will be discussed in greater detail in Chapter V. 

Therefore, the changes necessary to implement the Extended Kalman smoother 
are the following: 

a. Coordinate Conversion 

First, the innovations equation (Eqn. 3.3) must incorporate the coordinate 

conversion. 

z„ ( /2 = z, r - g(x£) (3.22) 

where, 
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(3.23) 



b. The Measurement Matrix 



The Kalman measurement matrix (Eqn. 3.5) will no longer be a constant 



matrix, but will have to be re-computed each time. 



c;:> =[§(*!:>)] 




(3.24) 



The same modifications are also necessary for the smoothing step. 



Therefore, (Eqns. 3.11-3.16) will be changed accordingly. Another effect of using the 
Extended Kalman is that the algorithm converges more quickly and easily than before. 
Therefore, the stopping criteria, Kc, can be lowered, producing relatively the same 
number of iterations, while allowing a tighter convergence parameter. For the Extended 
Kalman algorithm, Kc was set to 10' 8 . 

3. Stricter Speed Limit 

As different geometries were simulated, I found that the original speed limit still 
allowed targets to “run away,” i.e., have a velocity estimate which was too fast. 
Therefore, a modification to the limit on the velocity of the predicted track estimate was 
necessary. 

The stricter speed limit is implemented by first selecting the middle time scan 
estimate as the baseline. If this estimate has a speed in excess of 10 knots, then the 
velocity of the baseline is reduced to reflect a speed of 1 0 knots. Then the state estimates 
at both past and future times are generated from the adjusted baseline state with its new 
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refined velocity. These estimates are then used in the next batch of processing. Even 
with this stricter speed limit, the algorithm is still allowed to converge to a final track 



estimate with any velocity. 

4. Attribute Data 

The use of measurement attribute data was explored and added to the algorithm. 
This method assumes that a measure of target amplitude was available in addition to the 
range-bearing measurement. The measured amplitude is drawn from a Rayleigh 
distribution with specified mean, which has the form: 



The mean for this distribution is 12 . For this research, targets and clutter are 
assigned different means. The clutter model is given a mean with a= 1, and the target 
models have means with a> 1. This data is included by multiplying the weights found in 
(Eqns. 3.1 & 3.17) by the Rayleigh distribution. Therefore, (Eqns. 3.1 & 3.17) become 



respectively. This modification was only used in the simulations that are indicated in 
Chapter V. 

A block diagram with all of the modifications, except the attribute data, is shown 
in Figure 3.3. 



f(o) = (a / <j 2 )exp(-a 2 / 2cr ), a > 0 (3.25) 




(3.26) 




(3.27) 
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iterate 




This concludes the layout of the PMHT algorithm, which is utilized in this 
research. In the next chapter, I will discuss the initialization routine that is used for this 
algorithm. 
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IV. TARGET STATE INITIALIZATION 



A. GENERAL DISCUSSION 

The initialization algorithm used in this research is of the N-of-N variety. That is, 
for the first N time scans, there must be N measurement points which meet the gating 
criteria. The PMHT algorithm is very sensitive to the initial estimates it is given. For 
small values of N, the initial heading can be extremely inaccurate, and then the algorithm 
is less likely to converge to the true track. Therefore, I have found that values of N > 5 
are necessary for the PMHT algorithm to perform reliably. 

B. THE N-of-N INITIALIZATION ALGORITHM 

There are two steps for this process when N > 3 . The first step is a gating 
equation which eliminates all points which are not likely to be associated as a track. 

Then once a set of N points has been designated as a new track, a least squares algorithm 
is used to fit the optimal track to these N points. 

1. Gating 

As has been stated before, this research assumes that targets have a speed between 
2 and 10 knots. If the target velocity vector is known and two successive measurements 
(z, and z, +1 ) are obtained for the target, then the quantity 

x 2 = U„, -Z,) T [R„, +R,r'(z„, -Z,) (4.1) 
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has a non-central Chi-squared distribution, where z, and R, are the measured track 
position and measurement covariance matrix at time t. It was empirically demonstrated 
that for the target speeds, tracking ranges, and measurement errors used in this research, a 

cut-off of x 2 ^ 50 kept all target associations. This cut-off produces an effective gate for 
2-of-2 association of approximately 75 square kilometers. A 3-of-3 case relies on a 
match (using the 2-of-2 algorithm) for the first and second measurements and the second 
and third measurements. Then a cut-off value for track formation was applied with 

X 2 < 20 . This gives an association probability for measurements derived from a real 
track of 0.9972. Higher order associations project the subsequent result into the future to 
determine likely measurements for the new association. At each projection for the next 
higher association, a match is performed and then a x rejection. 

2. Least Squares Fit 

This initialization algorithm uses a least squares fit assuming a constant velocity 
target during the initialization period. Therefore, for the 3-of-3 case, the first three 
measurements are given by: 



Z 1 m ~ CX]„, + e i r 



z 2 ,„ = Cx 2 „ + e 2r = C[0x„„ + WjJ + e 2r 
Z 3 w = Cx 3 „, + e 3r = C[d>(0x lra + w,J + w 2 J + e 3r 



(4.2) 



Since a straight, constant velocity target is assumed during the initialization, the process 
errors are set to zero (i.e.. w /m =0 for ever) 7 /). Therefore, 
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(4.3) 



z u„ 




* 1 , 


Z 2n, 


= Fx + 


e 2r 


1 

N 

Ui 

L 


6*1 


- e 3r. 



where. 



F = 



H 

HO 

HOO 



where the covariance associated with the error vector is 



< 


1 

i £ 

<y 

i 


k 


> 

e 2r e 3r] 


> — 


1 


0 

R 2r 


0 

0 




1 

V. 

ro 

<L> 

1 








1 

o 


0 


R 3,_ 



(4.4) 



(4.5) 



Therefore, the least squares estimate of x ]m is given by: 



x,.,=[F T 2'rF]' , F t Z i ; 

and 

( 4 - 7 ) 

since this gives the estimates for the position. For the associated state estimate 
covariance matrices, the following equations are used: 



J lm 



2 m 



^3 m. 



(4.6) 



Pi- = [F T SiF]' I F T ([F T £^F]' , F T Zi) T (4.8) 

and 

P 3m = OP 2 „,O t + Q = o(oP lm O T + Q)o T + Q (4.9) 



25 



From this 3-of-3 case, it is straightforward to expand this in order to apply this process to 
the 5-of-5 case. These estimates and their associated covariance matrices are then used in 
the first batch of the PMHT algorithm. 
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V. RESULTS AND COMPARISONS 



A. SIMULATIONS 

In this research, all of the comparisons are based on simulated data. Simulations 
were run for 30 time scans with the time between scans equal to 4 minutes. The 30 
range-bearing target measurements are generated using additive Gaussian noise, where 
the range standard deviation is 100 meters and the bearing standard deviation is 3 
degrees. An example of a simulated target run is shown in Figure 5.1 . 



x 10 4 




Figure 5.1. Straight Track with Clutter 
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The solid line is the true track, and the are the noisy measurements. The 
actual target velocity is 5 knots in all scenarios. The sensor is located at (0,0), so the 
target is moving predominantly in the cross range dimension at a range greater than 40 
kilometers. 

This example also depicts clutter points as circles. Clutter was generated in a 
uniform random fashion throughout the area of interest. For this example the clutter 
density is 1 .67x1 O' 2 clutter points per square kilometer, or in other words, there are five 
clutter points for each time scan. Clutter densities in this study were varied from 
3.33xl0' 3 to 6.67xl0' 2 clutter points per square kilometer. 

B. OTHER ALGORITHMS 

The MHT and PDAF algorithms were used as a benchmark to see how well the 
PMHT is performing. As was mentioned in Chapter I, both of these are established 
algorithms that are currently being used in tracking applications. The results from these 
algorithms were produced using the exact same scenarios as were used for the PMHT. 
The MHT and PDAF algorithms used are widely discussed in several different texts (e.g., 
Bar-Shalom and Li [Ref. 4], Blackman [Ref. 5]). 

C. RESULTS 

Three different target motion geometries were tested during this research. First, 
there is the basic straight line track as is depicted in Figure 5. 1 . Second, two crossing 
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tracks are used moving in straight lines with constant velocity. The third geometry is a 
track which makes a turn half way through the simulation. 

The PMHT algorithm was first tested in each of the three geometries with a low 
clutter density (3.33x1 0' ? clutter points per square kilometer). Then it was confirmed 
with higher clutter densities and compared to the competing algorithms. 

1. Straight Line Target Motion 

Figure 5.2 displays a typical converged result produced by the PMHT algorithm. 



x 10 4 




Figure 5.2. Typical PMHT Result 



29 



On this single, straight line track, the circles are the actual target positions at each time 
increment, and the symbols are the smoothed, converged estimates. The clutter 
density for this simulation is 3. 33x1 O' 3 clutter points per square kilometer. 
a. Low Clutter Density 

In order to quantify the results, mean distance errors were computed for 
the final target estimates at each time scan. The means were taken from 500 simulation 
runs. Figure 5.3 shows these mean distance errors for a straight line track in a clutter 
density of 3.33x1 O' 3 clutter points per square kilometer. 




Figure 5.3. Measurement and Estimate Errors (low clutter) 
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The lowest dotted line curve on the figure is the result of running the 
Kalman Smoother using the actual target measurements (i.e., no clutter) and computing 
the mean distance errors over 1 000 simulation runs. Therefore, this curve represents an 
approximate theoretical minimum for algorithm performance in the absence of clutter. 
The highest curve on the figure (the dotted line) is the average, noisy measurement error 
computed over 1000 runs. Clearly, an algorithm should be below this line to be 
considered as a viable option. The other line on the figure (the solid line) is the mean 
estimate error, which was produced by the algorithm. This is the average estimate error 
for 500 simulation runs at a clutter density of 3. 33x1 0' 3 clutter points per square 
kilometer. 

In this simulation, the algorithm happened to produce estimate errors 
which have a better mean than the Kalman Smoother without clutter. This can be true 
because of randomness, but in general will not happen. However, it does show that the 
PMHT algorithm is performing extremely well in low clutter, as would be expected. 

b. Medium Clutter Density 

Figure 5.4 shows the results using the PMHT algorithm in a medium 
clutter density. The medium clutter density has five clutter points and a noisy 
measurement in the search area at each time scan. The clutter density is 1 .67x1 0‘ 3 clutter 
points per square kilometer. The solid line in Figure 5.4 displays the results from using 
the Extended Kalman Smoother (EKS) in the algorithm. The broken line shows the 
results of using the conventional Kalman Smoother in Cartesian Coordinates, using the 
debiased equations. As can be seen, the Extended Kalman Smoother performed better. 
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Furthermore, the EKS performed the 500 simulations in only 204 minutes, whereas, the 
conventional Kalman algorithm took 280 minutes. Therefore, from these results, I have 
determined for this application that it is better to use the EKS as opposed to the 
conventional Kalman with the debiased equations. From here on, I will only show results 
from the EKS, but at other clutter densities, the results are similar between the 
conventional Kalman and the EKS, as is shown here. 




Figure 5.4. Comparison between EKS and Conventional Kalman 



Figure 5.5 shows the comparison between the nearest neighbor (NN), 



PMHT, MHT, and PDAF. On this figure, there are four different lines — one for each of 



the different algorithms. The solid line represents the PMHT; the solid line with circles is 
the nearest neighbor; the broken line is the MHT; and the dash-dot line is the PDAF. The 
important estimate error to compare is at the last time scan. This is the time that is 
current and is of importance. The PMHT will almost always have better errors for earlier 
time scans because of the smoothing process. For this clutter density, the PMHT is 
performing slightly better than the MHT and considerably better than the PDAF and the 
nearest neighbor. This is especially promising given the fact that the PMHT is less 
complicated and easier to compute than the MHT. Since the nearest neighbor algorithm 
is clearly not a viable option, it is not included on subsequent plots. 




Figure 5.5. Comparison between NN, PMHT, PDAF, and MHT (medium clutter) 



c. High Clutter Density 

For a high density clutter environment, a clutter density of 3.33xl0' 2 
clutter points per square kilometer was used. This meant that there were 10 clutter points 
and one noisy measurement in the search area per time scan. The results from this 
simulation are shown in Figure 5.6. 




Figure 5.6. Comparison between PMHT, PDAF, and MHT (high clutter) 



These results show that the PMHT is performing better than the PDAF, 
but worse than the MHT for this clutter density. Therefore at this point, there is a cost 
trade-off versus the algorithm performance. The MHT performs the best, but it also is the 
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most expensive in terms of computations. On the other hand, the PDAF is producing the 
worst results, but it is the cheapest and fastest to calculate. 

2. Turning Tracks 

For this scenario, the target moved in a straight line with constant speed for the 
first 60 minutes. At that point, the target made a 10 degree turn and then continued in a 
straight line for the remaining time. This target motion and a typical converged result 
produced by the PMFIT algorithm are displayed in Figure 5.7. 



x 10 4 




Figure 5.7. Typical PMHT Results with a Turning Track 
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Again, the circles are the actual target positions at each time increment, and the 
symbols are the smoothed, converged estimates. The clutter density for this 
simulation is 3.33x1 O' 3 clutter points per square kilometer. This result shows that the 
algorithm is not handling the turn quite as well as the straight line track; however, it is 
tracking nonetheless. For this 10 degree turn, the q factor was set to a value of one. 
a. Low Clutter Density 

Figure 5.8 displays the mean errors for a turning track for 500 simulation 
runs. The solid line represents the estimate errors at each time scan. 




Figure 5.8. Measurement and Estimate Errors (low clutter) 



36 



The clutter density for this simulation is 3.33x1 O' 3 clutter points per square 
kilometer. While the errors for this scenario are higher than for the straight track, they 
are still well below the measurement errors. 

b. Medium Clutter Density 

Figure 5.9 shows the results of the PMHT algorithm on a turning track in a 
clutter density of 1 .67x1 O' 2 clutter points per square kilometer. These results show that 
the algorithm is still performing well even with the target making a turn in the presence 
of a medium clutter density. This further justifies the use of the q factor as a small, but 
positive value. For this scenario, q was set to a value of 10. 




Figure 5.9. Measurement and Estimate Errors (medium clutter, 10° turn) 
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Figure 5.10 shows the results of the three algorithms estimating a turning 
track with a 90 degree turn instead of a 1 0 degree turn. Again, the solid line represents 
the PMHT; the broken line is the MHT; and the dash-dot line is the PDAF. The clutter 
density is still 1.67xl0‘ 2 clutter points per square kilometer. 




Figure 5.10. Comparison between PMHT, PDAF, and MHT (90° turn) 



Here the q factor was set to a value of 300 in an effort to try and get the 
PMHT algorithm to track through the turn. However, with a turn this radical, the PMHT 
and PDAF algorithms were unable to maintain the target. The MHT results show that the 
target was lost at the turn, but was able to be reacquired after the turn. The MHT 
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algorithm has reacquisition logic inherent in the algorithm itself, while the other two 
algorithms do not have this process inherent to the basic tracking filter. Therefore, this 
result leads to the conclusion that the use of the PMHT will require that some sort of 
target re-initialization be implemented in a fielded system. A procedure for linking the 
new track with the old would also be required. 

c. High Clutter Density 

Figure 5.11 shows the results of the PMHT tracking a target through a 10 
degree turn in a clutter density of 3.33xl0' 2 clutter points per square kilometer. 




Figure 5.11. Measurement and Estimate Errors (high clutter, 1 0° turn) 
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These results confirm what was seen at the medium clutter density — 
namely that the PMHT algorithm is able to robustly follow a track as it makes small 
maneuvers. This justifies further using the small, but non-zero q factor value instead of 
setting it to zero, which would be the equivalent of using a least squares fit instead of a 
Kalman filter or smoother. 

3. Crossing Tracks 

In this scenario, there are two targets in the simulation. Target one executes the 
same track that was used in the straight track from before. Target two executes a track 
which starts in the bottom left-hand comer of the figure and runs toward the upper right- 
hand comer. Therefore, this new track is predominantly in the cross bearing direction. 
These tracks along with typical converged results from the PMHT algorithm are shown in 
Figure 5.12. 

Again, the actual target positions are the circles, and the symbols are the 
smoothed, converged estimates. The clutter density for this simulation is 3.33xlO' 3 
clutter points per square kilometer. Initially, there were some problems with the 
algorithm tracking the different trajectories, but since the improved limit on velocity was 
implemented, this has not been a drawback. This typical result shows the effect of the 
greater uncertainty in the bearing dimension. Track one, which is predominantly moving 
in the cross-range direction, has a greater uncertainty along its axis of travel. On the 
other hand, track two, which is predominantly moving in the cross-bearing direction, has 
a greater uncertainty perpendicular to its axis of travel. 
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Figure 5.12. Typical PMHT Results with Crossing Tracks 
a. Low Clutter Density 

Figure 5.13 displays the results from the crossing track scenario with a low 
clutter density of 3.33x1 0‘ 3 clutter points per square kilometer. The solid line shows the 
mean errors from track one, and the broken line is from track two. Again, the lowest 
dotted line represents the theoretical minimum, which was produced by running the 
Kalman smoother with just the noisy measurements. The highest line is the average 
measurement error over 1000 simulations. These results are still below the measurement 
error, but significantly higher than the single track in a low clutter density. 
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Figure 5.13. Errors for Crossing Tracks (low clutter) 

One reason for this is the effect that each track has on the other one. The 
clutter has an equal probability of affecting the estimate to one side or the other, and, in 
general, clutter will tend to have little bias effect on the track estimate. On the other 
hand, the measurements from the other track will tend to pull the estimate toward these 
measurements, causing a bias to one side. Since, in this low clutter density the clutter has 
minimal effect, this bias effect of the other track is clearly evident. Figure 5.14 shows 
this bias effect. This figure is the average track estimate, which is produced by the 
algorithm over the 500 runs. 
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Figure 5.14. Average Track Estimate (low clutter) 



The true target tracks are shown as the solid lines, and the estimate 
average is displayed as the dotted lines. Track one is pulled downward, and track two is 
pulled up towards track one. In the next subsection, I will show how more clutter will 
dampen out this bias effect. 

b. Medium Clutter Density 

Figure 5.15 shows the results from the crossing track scenario in a medium 
clutter density of 1 .67x1 O' 2 clutter points per square kilometer. Once again, the solid line 
represents the mean errors from track one, and the broken line is from track two. 
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Figure 5.15. Errors for Crossing Tracks (medium clutter) 



Even though the clutter density is five times greater, these results are 
significantly better than the low clutter density case. This is due to the higher clutter 
density dampening out the bias caused by the two tracks. These results compare very 
closely to those obtained for the single straight track in a medium clutter density. 

Figure 5.16 shows the average track estimate for this scenario as was 
depicted in Figure 5.14 for the low clutter density. Again, the true target tracks are 
shown as the solid lines, and the estimate averages are displayed as the dotted lines. 
Indeed, the separate tracks show very' little bias toward the other track in the simulation. 
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Figure 5.16. Average Track Estimate (medium clutter) 
c. High Clutter Density 

Figure 5.17 shows the results from the crossing track scenario in a high 
clutter density. Again, the solid line displays the mean error from track one, and the 
broken line represents the mean errors from track two. The clutter density for this 
scenario is 3.33x1 O' 2 clutter points per square kilometer. For this simulation, the errors 
are starting to get up close to the measurement errors, and for clutter densities higher than 
this, the errors begin to exceed the measurement errors. 
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Figure 5.17. Errors for Crossing Tracks (high clutter) 



Another observation from these results is the fact that track one’s estimate 
is higher than that of track two. For the higher clutter densities, this was found to be the 
norm. The reason track one produces the higher estimate errors is due to the target 
motion being predominantly in the cross-range direction. Therefore, this is the reason 
track one was used for the majority of the simulations. 

4, Attribute Data 

The use of attribute data was researched to determine what level amplitude of 
target data, in relation to the clutter data amplitude, was necessary in order to improve the 
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performance of the PMHT algorithm. For this simulation, a very high clutter density of 
6.67xl0' 2 clutter points per square kilometer was used. Figure 5.18 shows the results of 
the PMHT algorithm in this clutter density with and without attribute data. 




Figure 5.18. Attribute Data Comparison (very high clutter) 

The solid line represents the algorithm with the use of attribute data, and the 

broken line is the algorithm without it. For this simulation an attribute value of a = VTo 
was necessary in order to get a noticeable improvement from the algorithm. As was 
described in Chapter III, this means that this value of a was used for the target 
measurements, and a value of a — 1 was used for the clutter measurements. For values 
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less than a - VTo , no clear advantage could be seen. The fact that a 1 OdB power 
advantage is needed for the target data in order to show an improvement is not very 
encouraging at this point. Given the findings of this research, a large amplitude 
separation would be necessary to produce any sort of real advantage using the attribute 
data. 

Attribute data was also utilized to see if it could lower the requirements for 
initialization. Figure 5.19 shows the effect of varying the initialization constant N, which 
is in N-of-N. A lOdB power ratio is used throughout these simulations. 




Figure 5.19. Mean Distance Errors with Attribute Data and Varying N 



48 



The solid line is the standard PMHT with no attribute data and N= 5. The dotted 



line is the algorithm with attribute data and N=3, and the dash-dot line represents attribute 
data with N= 5. The clutter density for these simulations is a high clutter density of 
3. 33x1 O' 2 clutter points per square kilometer. Here the attribute data with N= 3 is not as 
good as no attribute processing and N= 5. However, attribute data with N=5 shows 
superior results. Therefore, at higher clutter levels, it is not possible to reduce N and 
make up the difference with lOdB of attribute information. 
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VI. CONCLUSIONS 



A. SUMMARY 

This research has explored the possible implementation and initialization of the 
PMHT algorithm. It has solved and improved many of the aspects of running the 
algorithm. This includes the development of an initialization routine, a clutter weight, a 
cut-off for very small value weights, processing in five-scan batches, a maximum 
allowable velocity of the initial state estimate, clutter weight model inflation, the use of 
the Extended Kalman smoother, and the use of attribute data. 

The results from this research have shown that the PMHT algorithm is a viable 
player in the data association and tracking arena. It has been shown to outperform the 
PDAF in all of the scenarios studied here. Furthermore, it has proven to be superior to 
the MHT in low clutter densities, although it is not as good in the high clutter densities. 
The PMHT has also shown that it can track a target through a minor turn. Even though it 
will not track through a radical target maneuver, this is not a glaring weakness since most 
algorithms require special processing to track a target through a turn, as was discussed in 
Chapter V. 

Presently, the algorithm’s greatest shortcoming is in the area of initialization. The 
requirement for five measurements to line up (N=5) is stringent, especially when the 
probability of detection is less than one. Unfortunately, the PMHT algorithm has proven 
to be quite sensitive to the initial estimates. In addition, the algorithm is also easily 
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modified to make use of attribute data. However, the current 1 OdB signal to noise ratio is 
rather high, and probably is not obtainable in the underwater sonar world. 

B. FURTHER RESEARCH 

The PMHT algorithm has developed quickly since it was proposed by Streit and 
Luginbuhl in 1995. However, there are still several areas in which improvements and 
further research need to be addressed. Clearly, the initialization problem needs 
development, particularly in de-sensitizing the algorithm to the initial estimates. 

Another area for further research is the use of attribute data. The current use 
offers some promise, but more probability research needs to be done with the Rayleigh 
distribution in order to lower the KMB signal to noise ratio. Furthermore, using attribute 
data during initialization needs to be studied more. 

The final area where more research is needed is the processing of the algorithm 
during radical turns and maneuvers. This has been done successfully with other tracking 
algorithms, but actual simulations with the PMHT algorithm in linking tracks together 
would be useful. Investigation of the requirements necessary for the new estimates after 
the turn would be important. 
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APPENDIX. MATLABCODE 



This appendix contains the code which was used in Matlab 4.2c 1 to simulate the 
PMHT algorithm. The first set of code is for the two crossing target scenario. The 
second set of code is for a straight track with attribute data. For the other scenarios, slight 
modifications were made to either of these programs. 



Crossing Tracks 

%Probabilistic Multi-Hypothesis Tracking (version 36) 

% thesis by Capt. Darin T. Dunham, USMC 
% advisor: Prof. R. Gary Hutchins, NPS 
% 

% two tracks with clutter-- 
% first two meas are one std error, 

% the other meas are uniform clutter. 

% 

% Uses clutter tracking model. 

% 

% Tracks in Tstep scan increments. 

% 

% Uses attribute data for each measurement NOT IMPLEMENTED 
% which is a random Rayleigh distribution. 

% 

% Tracks cross. 

% 

% Uses first 5 points to initialize. 

% 

% Uses an Extended Kalman instead of debiased eqns. 

% 

% Computes the mean track to show bias of second track. 

% 



1 Matlab® copyright© 1984-94 The MathWorks, Inc., All Rights Reserved, Version 4.2c, Nov. 23, 1994. 
The MathWorks, Inc., 24 Prime Park Way, Natick, MA 01760. 
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clear 

tic 



%Constants 
M=3; 

N=5; 

Nt=2; 

Nc=10; 

T=30; 

Tstep=5; 
dt=4; 

1=100; 
sr=100; 
sth=pi/60; 
s=l; 
sc=l; 

q=i; 

maxv 1 =308 .66667; 
maxv2=3 0 8 . 66667 ; 
thr=10.5966; 
stop=le-6; 

J=500; 

%Constant vectors 
cwt=le-10*[le-2 le-2 1111]; %clutter model weight 

%Initiation for actual track (meters) 

Xlinit=[30200, 92.6, 30400, -123.46667]'; 

X2init=[30200, 92.6, 16078, 123.46667]'; 

Yl=zeros(4,T); 

Y2=zeros(4,T); 

Yl(:,l)=Xlinit; 

Y2(:,l)=X2init; 

Q=q*[(dt A 3)/3 (dt A 2)/2 0 0; (dt A 2)/2 dt 0 0; 

0 0 (dt A 3)/3 (dt A 2)/2; 0 0 (dt A 2)/2 dt]; 

R=[sr A 2 0; 0 sth A 2]; 

A=[0 1 00; 0000; 000 1; 0000]; 

C=[l 0 0 0; 00 1 0]; 

Phi=eye(4) + A*dt; 
invPhi=eye(4) - A*dt; 
for t=l:T-l, 

Yl(:,t+l)=Phi*Yl(:,t); 

Y2(:,t+l)=Phi*Y2(:,t); 



%three models, two track, one clutter 
%number of points used to initialize 
%number of tracks 
%number of clutter points 
%number of scans 

%algorithm increment step size (change cwt) 

%in minutes 

%max number of iterations 
%std for range 
%std for bearing 

%std for attribute on a true target 
%std for attribute on clutter 
%Q coefficient 

%max allowable initial velocity for track 1(10 knots) 
%max allowable initial velocity for track 2(10 knots) 
%threshold for 3 std 
%convergence stopping parameter 
%number of loops thru the simulation 



54 



end 

Y 1 pol=xy2polar(C* Y 1 ); 

Y2po l=xy 2po lar(C * Y2) ; 

%Initialize error vectors 

errm=zeros(l,T); 

erre=zeros(l,T); 

errm2=zeros(l,T); 

erre2=zeros(l,T); 

count 1=0; 

count2=0; 

%initialize mean estimated track vectors 
Xlm=zeros(4,T); 

X2m=zeros(4,T); 

%Loop through simulation J times* *************************************** 
for j=l :J, 

j 

%Initialize tracker 

pl=0.2*ones(l,T); %pl(t)-prob that target 1 has a meas in time t 

p2=0.2*ones(l,T); 

p3=1.0*ones(l,T); 

%Generate range and bearing measurements 
Zlpol=Ylpol + sqrt(R)*randn(2,T); 

Z2pol=Y2pol + sqrt(R)*randn(2,T); 

%Convert measurements to cartesian using Debiased Eqns. 
mu=l - (exp(-sth A 2) - exp(-(sth A 2)/2)); 

Zl=(mu*eye(2))*polar2xy(Zlpol); 

Z2=(mu*eye(2))*polar2xy(Z2pol); 

Rs=[convert(Zlpol,sr,sth); convert(Z2pol,sr,sth)]; 

%Generate attribute data for targets 
Z=[Zlpol; raylmd(s,l,T); Z2pol; raylmd(s,l,T)]; 

%Generate clutter measurements 
for m=l :Nc, 

Zipol=xy2polar([1.5e4*rand(l,T)+2.8e4; 2e4*rand(l,T)+1.5e4]); 

Z=[Z; Zipol; raylmd(sc,l,T)]; 
end 
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%Build Zbar 
Zbarl=Z(l,:); 

Zbar2=Z(2,:); 

for v=4:3:3*(Nt+Nc), 

Zbar 1= [Zbar 1; Z(v,:)]; 

Zbar2=[Zbar2; Z(v+1,:)]; 
end 

%Initialize XI and X2 
Xl=zeros(4,T); 

F=[C; C*Phi; C*(Phi A 2); C*(Phi A 3); C*(Phi A 4)]; 

R 1 =form(Rs( 1:3,1)); 

R2=form(Rs(l :3,2)); 

R3=form(Rs(l :3,3)); 

R4=form(Rs( 1:3,4)); 

R5=form(Rs(l :3,5)); 

Sig=[Rl zeros(2,8); zeros(2,2) R2 zeros(2,6); 

zeros(2,4) R3 zeros(2,4); zeros(2,6) R4 zeros(2,2); zeros(2,8) R5]; 
invSig=inv(Sig); 

K=inv(F'*invSig*F)*F'*invSig; 

Zbar=[Zl(:,l); Zl(:,2); Zl(:,3); Zl(:,4); Zl(:,5)]; 

Xl(:,l)=K*Zbar; 

Pv 1 (:,2)=reshape((K*Sig*K'), 16,1); 

X2=zeros(4,T); 

R 1 =form(Rs(4 : 6, 1 )); 

R2=form(Rs(4 : 6,2)); 

R3=form(Rs(4:6,3)); 

R4=form(Rs(4:6,4)); 

R5=form(Rs(4:6,5)); 

Sig=[Rl zeros(2,8); zeros(2,2) R2 zeros(2,6); 

zeros(2,4) R3 zeros(2,4); zeros(2,6) R4 zeros(2,2); zeros(2,8) R5]; 
invSig=inv(Sig); 

K=inv(F'*invSig*F)*F'*invSig; 

Zbar=[Z2(:,l); Z2(:,2); Z2(:,3); Z2(:,4); Z2(:,5)]; 

X2(:,l)=K*Zbar; 

Pv2( : ,2)=reshape((K* Sig*K'), 16,1); 

%Limit initial velocity 
initvel 1 =sqrt(X 1(2,1 ) A 2 + X1(4,1) A 2); 
initve!2=sqrt(X2(2, 1 ) A 2 + X2(4,1) A 2); 
vfactor 1 =initvel 1 /maxv 1 ; 
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vfactor2=initvel2/maxv2; 
if vfactorl>l, 

X 1 (2, 1 )=X1 (2, 1 )/vfactor 1 ; 

X 1 (4 , 1 )=X 1(4,1 )/vfactor 1 ; 
end 

if vfactor2>l, 

X2(2, 1 )=X2(2, 1 )/vfactor2; 

X2(4, 1 )=X2(4, 1 )/vfactor2; 
end 

%Predict initial estimate for first five points 
for t=2:5, 

Xl(:,t)=Phi*Xl(:,t-l); 

X2( : ,t)=Phi * X2( : ,t- 1 ) ; 

Pvl(:,2*t)=reshape((Phi*formP(Pvl(:,2*(t-l)))*Phi' + Q), 16,1); 
Pv2(:,2*t)=reshape((Phi*formP(Pv2(:,2*(t-l)))*Phi’ + Q), 16,1); 
end 

U1=X1(:,1:5); 

U2=X2(:,1:5); 

%'l rack 111 five SC2H 
for Ti=5:Tstep:T, 

%Begin iterations 
for i=l:I, 

%store last target meas prob 

plp=pl; 

p2p=p2; 

p3p=p3; 

for t=l:Ti, 
n(t)=Nt+Nc; 

%compute weights 
for r=l:n(t), 

zt=Z((3 *r-2):(3 *r- 1 ),t)-xy2polar(C* X l(:,t)); 
Ck=Cekf(Xl(:,t)); 

Sig=Ck*reshape(Pvl(:,2*t),4,4)*Ck’ + R; 
den=2*pi*sqrt(det(Sig)); 
wl (r,t)=exp(-0. 5 * zf * inv( Sig) * zt)/den; 
if zt'*inv(Sig)*zt > thr, 
wl(r,t)=le-20; 
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end 



zt=Z((3*r-2):(3*r-l),t)-xy2polar(C*X2(:,t)); 

Ck=Cekf(X2(:,t)); 

Sig=Ck*reshape(Pv2(:,2*t),4,4)*Ck' + R; 
den=2 * pi *sqrt(det(Sig)) ; 
w2 (r,t)=exp(-0 . 5 * zf * inv(Si g) * zt )/ den ; 
if zt'*inv(Sig)*zt > thr, 
w2(r,t)=le-20; 
end 

w3 (r,t)=cwt(Ti/T step); 

sum 1 =(p l(t)*wl (r,t)+p2(t)*w2(r,t)+p3(t)*w3 (r.t)); 
w 1 (r,t)=w 1 (r,t)/ sum 1 ; 
w2(r,t)=w2(r,t)/ sum 1 ; 
w3(r,t)=w3(r,t)/suml ; 
end 

%compute mean meas weight for target m at time t 
w 1 m(t)=( 1 /n(t))* sum( w 1 ( : ,t)) ; 
w2m(t)=( 1 /n(t))* sum(w2(: ,t)) ; 
w3 m(t)=( 1 /n(t)) * sum( w3 ( : ,t)) ; 

%update target meas prob 
p 1 (t)=w 1 m(t)*p 1 (t); 
p2(t)=w2m(t)*p2(t); 
p3(t)=w3m(t)*p3(t); 

%compute target meas centroid 
W l=wl (:,t)/(n(t)* wl m(t)); 
W2=w2(:,t)/(n(t)*w2m(t)); 

Z1 hat( : ,t) = [ W 1 '* Zbar 1 (: ,t); W 1 ' *Zbar2(: ,t)] ; 

Z2hat( : ,t)= [ W2' * Zbar 1 ( : ,t) ; W2'*Zbar2(:,t)]; 

end 

%run Extended Kalman smoother 

ylhat(:,l)=Xl(:,l); 

y2hat(:,l)=X2(:,l); 

%forward recursion 
for t=l:Ti-l, 

Pt=Phi*reshape(Pvl(:,2*t),4,4) !t! Phi' + Q; 
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Pvl(:,2*t+l)=Pt(:); 

y 1 hat( : ,t+ 1 )=Phi * y 1 hat( : ,t) ; 

k=n(t+l)*pl(t+l); 

Ck=Cekf(y 1 hat(:,t+ 1 )); 

G=k*Pt*Ck'*inv(k*Ck*Pt*Ck' + R); 

Pv 1 (: ,2*t+2)=reshape((Pt-G*Ck* Pt), 16,1); 

ylhat(:,t+l)=ylhat(:,t+l)+G*(Zlhat(:,t+l)-xy2polar(C*ylhat(:,t+l))); 

Pt=Phi*reshape(Pv2(:,2*t),4,4)*Phi' + Q; 

Pv2(:,2*t+l)=Pt(:); 
y2hat( : ,t+ 1 )=Phi * y 2hat( : ,t) ; 
k=n(t+l)*p2(t+l); 

Ck=Cekf(y2hat(:,t+l)); 

G=k*Pt*Ck'*inv(k*Ck*Pt*Ck' + R); 
Pv2(:,2*t+2)=reshape((Pt-G*Ck*Pt), 1 6, 1 ); 

y2hat(:,t+ 1 )=y2hat( :,t+ 1 )+G* (Z2hat( : ,t+ 1 )-xy2polar(C * y2hat( : ,t+ 1 ))); 
end 

Xl(:,Ti)=ylhat(:,Ti); 

X2(:,Ti)=y2hat(:,Ti); 

%backward recursion 
for t=Ti-l:-l:l, 

Ptt=reshape(Pv 1 (:,2*t),4,4); 
invPt=inv(reshape(P v 1 (: ,2 * t+ 1 ),4,4)); 

X 1 ( : ,t)=y 1 hat( : ,t)+Ptt* Phi' * invPt* (X 1 ( : ,t+ 1 )-Phi * y 1 hat( : ,t)) ; 

Ptt=reshape(Pv2(:,2*t),4,4); 

invPt=inv(reshape(Pv2(:,2*t+l),4,4)); 

X2(: ,t)=y2hat( :,t)+Ptt* Phi'* invPt* (X2( : ,t+ 1 )-Phi* y2hat( : ,t)) ; 
end 

%check for convergence 

diff=sum(abs(p 1 -p 1 p))+sum(abs(p2-p2p))+sum(abs(p3-p3p)); 
if diff<stop, 
break 
end 

end 

i 

%plot track output 
figure(l) 

plot(Y 1(1,1 :Ti),Y 1(3,1 :Ti),Xl ( 1 , 1 :Ti),Xl (3 , 1 :Ti), Y2( 1 , 1 :Ti),... 
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Y2(3 , 1 :Ti),X2( 1 , 1 :Ti),X2(3 , 1 :Ti)) 
axis('equal’), title(’Tracking Algorithm Output') 
xlabel('x (meters)'), ylabel('y (meters)') 

%Predict for next Tstep points if necessary 
if Ti<T, 

mid=floor(Ti/2); 

initvell=sqrt(Xl(2,mid) A 2 + Xl(4,mid) A 2); 
initvel2=sqrt(X2(2,mid) A 2 + X2(4,mid) A 2); 
vfactor 1 =initvel 1 /maxv 1 ; 
vfactor2=initvel2/maxv2; 
if vfactorl>l, 

XI (2,mid)=Xl (2,mid)/vfactorl ; 

Xl(4,mid)=Xl(4,mid)/vfactorl; 

end 

if vfactor2> 1 , 

X2(2,mid)=X2(2,mid)/vfactor2; 

X2(4,mid)=X2(4,mid)/vfactor2; 

end 

for t=mid+l :Ti+Tstep, 

Xl(:,t)=Phi*Xl(:,t-l); 

Pvl(:,2*t)=reshape((Phi*reshape(Pvl(:,2*(t-l)),4,4)*Phi'+Q),16,l); 
X2(: ,t)=Phi * X2(: ,t- 1 ); 

Pv2(:,2*t)=reshape((Phi*reshape(Pv2(:,2*(t-l)),4,4)*Phi'+Q),16,l); 

end 

for t=mid-l:-l:l, 

XI (:,t)=invPhi*Xl (:,t+l ); 

X2(:,t)=invPhi*X2(:,t+l ); 
end 
end 

end 

%update measurement & estimate errors for track 1 
errx=Yl(l,:)-Zl(l,:); 
erry= Y 1 (3 , : )-Z 1 (2, : ) ; 
errm=sqrt(errx. A 2 + erry. A 2)./J + errtn; 
errx=Yl(l,:)-Xl(l,:); ' 
erry=Yl(3,:)-Xl(3,:); 
errej=sqrt(errx. A 2 + erry. A 2); 
erre=errej ./J + erre; 
if max(errej) > 2000, 
count l=count 1+1; 
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end 



%update measurement & estimate errors for track 2 
errx=Y2( 1 ,:)-Z2( 1 
erry=Y2(3,:)-Z2(2,:); 
errm2=sqrt(errx. A 2 + erry. A 2)./J + errm2; 
errx=Y2( 1 ,:)-X2( 1 
erry=Y2(3,:)-X2(3,:); 
errej=sqrt(errx. A 2 + erry. A 2); 
erre2=errej./J + erre2; 
if max(errej) > 2000, 
count2=count2+l ; 
end 



%update mean estimated tracks 
Xlm=Xlm + Xl./J; 

X2m=X2m + X2./J; 



end 

%End J times 



loop 






%plot errors 
figure(2) 
load kserror 

plot(l :T,errm, 1 :T,erre,l :T,errm2, 1 :T,erre2, 1 :T,errl (1 :T),1 :T,err2(l :T)) 
title('Measurement Error & Estimate Error, over 1 00 runs') 
xlabel('time'), ylabel('distance (m)') 

figure(3) 

plot(Yl (1,1 :T),Y1(3,1 :T),Xlm(l, 1 :T),Xlm(3,l :T),Y2(1,1 
Y2(3, 1 :T),X2m( 1 , 1 :T),X2m(3, 1 :T)) 
axis('equal'), title('Tracking Algorithm Output’) 
xlabel(’x (meters)'), ylabel('y (meters)') 



time=toc/60; 

[time count 1 count2] 



Attribute Data 

%Probabilistic Multi-Hypothesis Tracking (version 35) 
% thesis by Capt. Darin T. Dunham, USMC 
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% advisor: Prof. R. Gary Hutchins, NPS 
% 

% one track with clutter— 

% first meas is one std error, 

% the other meas are uniform clutter. 

% 

% Uses clutter tracking model. 

% 

% Tracks in Tstep scan increments. 

% 

% Uses attribute data for each measurement 
% which is a random Rayleigh distribution. 

% 

% Uses first 5 points to initialize. 

% 

% Uses an Extended Kalman instead of debiased eqns. 
% 



%clear 

tic 



%Constants 

M=2; 

N=5; 

Nt=l; 

Nc=20; 

T=30; 

Tstep=5; 

dt=4; 

1 = 100 ; 

sr=100; 

sth=pi/60; 

%s=sqrt(10); 

sc=l; 

q=l; 

maxv=308. 66667; 

thr=10.5966; 

stop=le-8; 

J=500; 



%two models 

%number of points used to initialize 
%number of tracks 
%number of clutter points 
%number of scans 
%algorithm increment step size 
%in minutes 

%max number of iterations 

%std for range 

%std for bearing 

%std for attribute on a true target 

%std for attribute on clutter 

%Q coefficient 

%max allowable predict velocity (10 knots) 
%threshold for 3 std 
%convergence value for Pi 
%number of loops thru the simulation 



%Constant vectors 

cwt=le-10*[le-2 le-2 1111]; %clutter model weight 
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%initiation for actual track (meters) 

Xlinit=[30200, 92.6, 30400, -123.46667]'; 

%Xlinit=[30200, 92.6, 16078, 123.46667]'; 

Yl=zeros(4.T); 

Yl(:,l)=Xlinit; 

C=[l 00 0; 00 1 0]; 

Q=q*[(dt A 3)/3 (dt A 2)/2 0 0; (dt A 2)/2 dt 0 0; 

0 0 (dt A 3)/3 (dt A 2)/2; 0 0 (dt A 2)/2 dt]; 

R=[sr A 2 0; 0 sth A 2]; 

A=[0 1 00;0 0 00; 0 00 1; 0000]; 

Phi=eye(4) + A*dt; 
invPhi=eye(4) - A*dt; 
for t=l:T-l, 

Yl(:,t+l)=Phi*Yl(:,t); 

end 

Y 1 pol=xy 2polar(C * Y 1 ); 
errm=zeros(l,T); 
erre=zeros(l,T); 
count=0; 

%Loop through simulation J times* ************************************** 
for j=l:J, 

j 

%lnitialize tracker 

pl=0.2*ones(l,T); %pl(t)-prob that tar 1 has a meas in time t 
p2=1.0*ones(l,T); 

%Generate range and bearing measurements 
Zlpol=Ylpol + sqrt(R)*randn(2,T); 

%Convert measurements to cartesian using Debiased Eqns. 
mu=l - (exp(-sth A 2) - exp(-(sth A 2)/2)); 

Z 1 =(mu* eye(2))* (polar2xy(Z 1 pol)); 

Rs=convert(Z 1 pol,sr,sth); 

%Generate attribute data for target & measurement covariance 
Z=[Zlpol; raylmd(s,l,T)]; 

%Generate clutter measurements 
for m=l:Nc, 

Zipol=xy2polar( [ 1 .5e4*rand( 1 ,T)+2.8e4; 2e4* rand( 1 ,T)+ 1 .5e4]); 

Z=[Z; Zipol; raylmd(sc,l,T)]; 
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end 



%Build Zbar 
Zbarl=Z(l,:); 

Zbar2=Z(2,:); 

for v=4:3:3*(Nt+Nc), 

Zbarl=[Zbarl; Z(v,:)]; 

Zbar2=[Zbar2; Z(v+1,:)]; 
end 

%Initialize X 
Xl=zeros(4,T); 

F=[C; C*Phi; C*(Phi A 2); C*(Phi A 3); C*(Phi A 4)]; 

Rl=form(Rs(:,l)); 

R2=form(Rs( : ,2)) ; 

R3=form(Rs( : ,3 )); 

R4=form(Rs(:,4)); 

R5=form(Rs(:,5)); 

Sig=[Rl zeros(2,8); zeros(2,2) R2 zeros(2,6); 

zeros(2,4) R3 zeros(2,4); zeros(2,6) R4 zeros(2,2); zeros(2,8) R5]; 
invSig=inv(Sig); 

K=inv(F'*invSig*F)*F'*invSig; 

Zbar=[Zl(:,l); Zl(:,2); Zl(:,3); Zl(:,4); Zl(:,5)]; 

Xl(:,l)=K*Zbar; 

Pv(:,2)=reshape((K* Sig*K'), 1 6, 1 ); 

%Limit initial velocity 
i nitvel=sqrt(X 1 (2 , 1 ) A 2 + X1(4,1) A 2); 
vfactor=initvel/ maxv; 
if vfactor>l, 

X 1 (2. 1 )=X 1 (2, 1 )/vfactor; 

XI (4, 1 )=X 1(4,1 )/vfactor; 
end 

%Predict initial estimate for first five points 
for t=2:5, 

Xl(:,t)=Phi*Xl(:,t-l); 

Pv(:,2*t)=reshape((Phi*formP(Pv(:,2*(t-l)))*Phi' + Q),16.1); 
end 

U1=X1(:,1 :5); 

m step scan 
for Ti=5:Tstep:T, 
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%Begin iterations 
for i=l:I, 



%store last target meas prob 

plp=pl; 

p2p=p2; 

for t=l:Ti, 
n(t)=Nt+Nc; 

%compute weights 
for r=l:n(t), 

zt=Z((3 * r-2) :(3 *r- 1 ),t)-xy2polar(C*X 1 (: ,t)); 
Ck=Cekf(Xl(:,t)); 

Sig=Ck*reshape(Pv(:,2*t),4,4)*Ck' + R; 
den=2 * pi * sqrt(det(S ig)); 

w 1 (r,t)=ray lpdf(Z(3 * r,t), s) * exp(-0 . 5 * zt' * in v( S i g) * zt)/den; 
if zt'*inv(Sig)*zt > thr, 
wl(r,t)=le-20; 
end 

w2(r,t)=raylpdf(Z(3*r,t),sc)*cwt(Ti/Tstep); 
sum 1 =(p 1 (t) * w 1 (r,t)+p2(t) * w2(r,t)); 
w 1 (r.t)=w 1 (r,t)/ sum 1 ; 
w2(r,t)=w2(r,t)/suml ; 
end 

%compute mean meas weight for target m at time t 
w 1 m(t)=( 1 /n(t)) * sum( w 1 ( : ,t)) ; 
w2m(t)=(l/n(t))*sum(w2(:,t)); 

%update target meas prob 
p 1 (t)= w 1 m(t) * p 1 (t) ; 
p2(t)=w2m(t)*p2(t); 

%compute target meas centroid 
W =w 1 ( : ,t)/(n(t) * w 1 m(t)); 

Zhat( : ,t)= [ W' *Zbar 1 (: ,t) ; W’*Zbar2(:,t)]; 

end 



%run Extended Kalman smoother 
yhat(:,l)=Xl(:,l); 
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%forward recursion 
for t=l:Ti-l, 

Pt=Phi*reshape(Pv(:,2*t),4,4)*Phi' + Q; 

Pv(:,2*t+l)=Pt(:); 
yhat( : ,t+ 1 )=Phi * y hat( : ,t) ; 
k=n(t+l)*pl(t+l); 

Ck=Cekf(y hat( : ,t+ 1 )) ; 

G=k* Pt* Ck' * inv(k* Ck* Pt* Ck’ + R); 
Pv(:,2*t+2)=reshape((Pt-G*Ck*Pt),16,l); 
yhat( : ,t+ 1 )=yhat(: ,t+ 1 )+G* (Zhat(: ,t+l )-xy2polar(C*yhat( : ,t+ 1 ))) ; 
end 

Xl(:,Ti)=yhat(:,Ti); 

%backward recursion 
for t=Ti-l:-l:l, 

Ptt=reshape(Pv(:,2*t),4,4); 
invPt=inv(reshape(Pv(: ,2 * t+ 1 ),4,4)); 

X 1 (: ,t)=yhat( : ,t)+Ptt* Phi' * invPt* (X 1 ( : ,t+ 1 )-Phi * yhat(:,t)) ; 
end 

%check for convergence 
diff=sum(abs(p 1 -p 1 p))+sum(abs(p2-p2p)); 
if diff < stop, 
break 
end 

end 

U2=Xl(:,l:Ti); 



%plot track output 
figure(l) 

plot(Y 1(1,1 :Ti),Y 1(3,1 :Ti),X 1 ( 1 , 1 :Ti),X 1 (3, 1 :Ti)) 
title('Tracking Algorithm Output') 
xlabel('x (meters)'), ylabel('y (meters)') 

%Predict for next five points if necessary 
if Ti<T, 

mid=floor(Ti/2); 

initvel=sqrt(Xl(2,mid) A 2 + Xl(4,mid) A 2); 

vfactor=initvel/maxv; 

if vfactor>l, 
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X 1 (2,mid)=X 1 (2,mid)/vfactor; 

X 1 (4,mid)=X 1 (4,mid)/vfactor ; 
end 

for t=mid+l :Ti+Tstep, 

Xl(:,t)=Phi*Xl(:,t-l); 

Pv(:,2*t)=reshape((Phi*reshape(Pv(:,2*(t-l)),4,4)*Phi' + Q),16,l); 
end 

for t=mid-l:-l:l, 

X 1 (:,t)=invPhi*X 1 (:,t+ 1); 
end 
end 

end 

errx=Yl(l,:)-Z 1(1,:); 
erry=Yl(3,0-Zl(2,0; 
errm=sqrt(errx. A 2 + erry. A 2)./J + errm; 
errx=Yl(l,0-Xl(l,0; 
erry=Yl(3,0-Xl(3,0; 

errej=sqrt(errx A 2 + erry. A 2); 
erre=errej./J + erre; 
if max(errej) > 2000, 
count=count+l; 
end 

end 

%End J times loop*************************************************** 

%plot errors 
figure(2) 
load kserror 

plot( 1 :T,erre, 1 :T,err 1 ( 1 :T), 1 :T,err2( 1 :T)) 
title('Measurement Error & Estimate Error, over 100 runs') 
xlabel('time'), ylabel('distance (m)') 

vel=sqrt(U 1 (2, 1 ) A 2 + U1(4,1) A 2); 

anglediff=( 1 80/pi)*(atan2(U 1 (4, 1 ),U 1 (2, 1 ))-atan2( Y 1 (4, 1 ), Y 1 (2, 1 ))); 
time=toc/60; 

[vel anglediff time count] 
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